﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;

namespace CameraAPI
{
    public class Calibration
    {
        public struct camera_parameters
        {
            public double Ncx;		/* [sel]     Number of sensor elements in camera's x direction */
            public double Nfx;		/* [pix]     Number of pixels in frame grabber's x direction   */
            public double dx;		/* [mm/sel]  X dimension of camera's sensor element (in mm)    */
            public double dy;		/* [mm/sel]  Y dimension of camera's sensor element (in mm)    */
            public double dpx;		/* [mm/pix]  Effective X dimension of pixel in frame grabber   */
            public double dpy;		/* [mm/pix]  Effective Y dimension of pixel in frame grabber   */
            public double Cx;		/* [pix]     Z axis intercept of camera coordinate system      */
            public double Cy;		/* [pix]     Z axis intercept of camera coordinate system      */
            public double sx;		/* []        Scale factor to compensate for any error in dpx   */
        };
        public struct calibration_constants
        {
            public double f;		/* [mm]          */
            public double kappa1;		/* [1/mm^2]      */
            public double p1;		/* [1/mm]        */
            public double p2;		/* [1/mm]        */
            public double Tx;		/* [mm]          */
            public double Ty;		/* [mm]          */
            public double Tz;		/* [mm]          */
            public double Rx;		/* [rad]	 */
            public double Ry;		/* [rad]	 */
            public double Rz;		/* [rad]	 */
            public double r1;		/* []            */
            public double r2;		/* []            */
            public double r3;		/* []            */
            public double r4;		/* []            */
            public double r5;		/* []            */
            public double r6;		/* []            */
            public double r7;		/* []            */
            public double r8;		/* []            */
            public double r9;		/* []            */
        };
        public double sa, ca, sb, cb, sg, cg;
        public camera_parameters cp; 
        public calibration_constants cc;

        public void image_coord_to_world_coord(double Xfd, double Yfd, double zw, ref double xw, ref double yw)
        {
            double Xd, Yd, Xu, Yu, common_denominator;

            /* convert from image to distorted sensor coordinates */
            Xd = cp.dpx * (Xfd - cp.Cx) / cp.sx;
            Yd = cp.dpy * (Yfd - cp.Cy);

            /* convert from distorted sensor to undistorted sensor plane coordinates */
            Xu = 0; Yu = 0;
            distorted_to_undistorted_sensor_coord(Xd, Yd, ref Xu, ref Yu);

            /* calculate the corresponding xw and yw world coordinates	 */
            /* (these equations were derived by simply inverting	 */
            /* the perspective projection equations using Macsyma)	 */
            common_denominator = ((cc.r1 * cc.r8 - cc.r2 * cc.r7) * Yu +
                      (cc.r5 * cc.r7 - cc.r4 * cc.r8) * Xu -
                      cc.f * cc.r1 * cc.r5 + cc.f * cc.r2 * cc.r4);

            xw = (((cc.r2 * cc.r9 - cc.r3 * cc.r8) * Yu +
                (cc.r6 * cc.r8 - cc.r5 * cc.r9) * Xu -
                cc.f * cc.r2 * cc.r6 + cc.f * cc.r3 * cc.r5) * zw +
               (cc.r2 * cc.Tz - cc.r8 * cc.Tx) * Yu +
               (cc.r8 * cc.Ty - cc.r5 * cc.Tz) * Xu -
               cc.f * cc.r2 * cc.Ty + cc.f * cc.r5 * cc.Tx) / common_denominator;

            yw = -(((cc.r1 * cc.r9 - cc.r3 * cc.r7) * Yu +
                 (cc.r6 * cc.r7 - cc.r4 * cc.r9) * Xu -
                 cc.f * cc.r1 * cc.r6 + cc.f * cc.r3 * cc.r4) * zw +
                (cc.r1 * cc.Tz - cc.r7 * cc.Tx) * Yu +
                (cc.r7 * cc.Ty - cc.r4 * cc.Tz) * Xu -
                cc.f * cc.r1 * cc.Ty + cc.f * cc.r4 * cc.Tx) / common_denominator;
        }

        public void distorted_to_undistorted_image_coord(double Xfd, double Yfd, ref double Xfu, ref double Yfu)
        {
            double Xd, Yd, Xu, Yu;

            /* convert from image to sensor coordinates */
            Xd = cp.dpx * (Xfd - cp.Cx) / cp.sx;
            Yd = cp.dpy * (Yfd - cp.Cy);

            /* convert from distorted sensor to undistorted sensor plane coordinates */
            Xu = 0; Yu = 0;
            distorted_to_undistorted_sensor_coord(Xd, Yd, ref Xu, ref Yu);

            /* convert from sensor to image coordinates */
            Xfu = Xu * cp.sx / cp.dpx + cp.Cx;
            Yfu = Yu / cp.dpy + cp.Cy;
        }

        void distorted_to_undistorted_sensor_coord(double Xd, double Yd, ref double Xu, ref double Yu)
        {
            double distortion_factor;
            /* convert from distorted to undistorted sensor plane coordinates */
            distortion_factor = 1 + cc.kappa1 * ((Xd * Xd) + (Yd * Yd));
            Xu = Xd * distortion_factor;
            Yu = Yd * distortion_factor;
        }

        public void load_calib_data(String filenmae)
        {
            StreamReader file = new StreamReader(filenmae);
            cp.Ncx = double.Parse(file.ReadLine());
            cp.Nfx = double.Parse(file.ReadLine());
            cp.dx = double.Parse(file.ReadLine());
            cp.dy = double.Parse(file.ReadLine());
            cp.dpx = double.Parse(file.ReadLine());
            cp.dpy = double.Parse(file.ReadLine());
            cp.Cx = double.Parse(file.ReadLine());
            cp.Cy = double.Parse(file.ReadLine());
            cp.sx = double.Parse(file.ReadLine());

            cc.f = double.Parse(file.ReadLine());
            cc.kappa1 = double.Parse(file.ReadLine());
            cc.Tx = double.Parse(file.ReadLine());
            cc.Ty = double.Parse(file.ReadLine());
            cc.Tz = double.Parse(file.ReadLine());
            cc.Rx = double.Parse(file.ReadLine());
            cc.Ry = double.Parse(file.ReadLine());
            cc.Rz = double.Parse(file.ReadLine());

            sa = Math.Sin(cc.Rx); ca = Math.Cos(cc.Rx);
            sb = Math.Sin(cc.Ry); cb = Math.Cos(cc.Ry);
            sg = Math.Sin(cc.Rz); cg = Math.Cos(cc.Rz);

            cc.r1 = cb * cg;
            cc.r2 = cg * sa * sb - ca * sg;
            cc.r3 = sa * sg + ca * cg * sb;
            cc.r4 = cb * sg;
            cc.r5 = sa * sb * sg + ca * cg;
            cc.r6 = ca * sb * sg - cg * sa;
            cc.r7 = -sb;
            cc.r8 = cb * sa;
            cc.r9 = ca * cb;

            cc.p1 = double.Parse(file.ReadLine());
            cc.p2 = double.Parse(file.ReadLine());
        }
    }
}
